perm filename STKDRV.SAI[SYS,HE] blob
sn#004283 filedate 1972-08-25 generic text, type T, neo UTF8
COMMENT ⊗ VALID 00017 PAGES
RECORD PAGE DESCRIPTION
00001 00001
00003 00002 BEGIN "STKDRV"
00007 00003
00011 00004 SIMPLE PROCEDURE ADAPT_ARM(REAL XH,YH,ZHREFERENCE REAL XER,YER)
00014 00005 PROCEDURE FIND_CUBE(INTEGER CUB,BKGR,INT REAL XTC,YTC)
00020 00006 PROCEDURE GRSP_CUBE(INTEGER CUB)
00023 00007 FIND_HAND(XT1,YT1,XT2,YT2,XER,YER,TRUE)
00027 00008 PROCEDURE STAK_CUBE(INTEGER BOT,TOP REAL DELM,DELS)
00030 00009 WAIT
00034 00010 KHC: WID←RCUBES[TOP,6]/2 HIT←3*WID/2-RHAND[2]
00038 00011 IF ABS(DIRD2[1])<.707
00041 00012 DUM←DIRD3[1]*DIRD4[2]-DIRD3[2]*DIRD4[1]
00045 00013 PROCEDURE PLAS_CUBE(INTEGER CUBREAL XTC,YTC)
00049 00014 KHC: WID←RCUBES[CUB,6]/2 HIT←3*WID/2-RHAND[2]
00053 00015 IF ABS(DIRD2[1])<.707
00057 00016 REAL XTC,YTC,DELM,DELS
00061 00017 OUTSTR(CVS(HCUBE)CRLF)
00062 ENDMK
⊗;
BEGIN "STKDRV"
REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
REAL ARRAY RCUBES[1:8,1:12],RTABL[1:10,1:10,1:2],RHAND[1:2];
INTEGER ARRAY ICUBES[1:8,1:5],ITABL[1:10,1:10];
INTEGER MESS,STKFLG,HCUBE;
REAL P0X,P0Y,GCOR,MARKX,MARKZ,THICK;
BOOLEAN DBUG;
DEFINE CRLF="&'15&'12"; DEFINE YES="INCHWL=""Y"""; DEFINE ⊃="COMMENT";
DEFINE WAIT="IF DBUG THEN BEGIN OUTSTR(""...TYPE Y TO CONTINUE:""CRLF);
IF INCHWL=""Y"" THEN; END";
DEFINE S_O_T="STOP_ON_TOUCH";
⊃ MVVF is "Most" Visible Vertical Face when the camera is centered on the cube;
⊃ SVVF is 2nd. Visible Vertical Face when the camera is centered on the cube;
⊃ RCUBES[.,1]-X cor. of center of cube;
⊃ RCUBES[.,2]-Y cor. of center of cube;
⊃ RCUBES[.,3]-Z cor. of center of cube;
⊃ RCUBES[.,4]-x comp of the normal to MVVF;
⊃ RCUBES[.,5]-Y comp of the normal to MVVF;
⊃ RCUBES[.,6]-size of cube;
⊃ RCUBES[.,7]-M, sine of angle between normal to MVVF and camera l.o.s.
M<0 if MVVF is on the left side, M>0 if MVVF is on the right;
⊃ RCUBES[.,8]-X coord in arm reference for grasping again;
⊃ RCUBES[.,9]-Y coord in arm reference for grasping again;
⊃ RCUBES[.,10]-Z coord in arm reference for grasping again;
⊃ RCUBES[.,11]-x comp of the normal to SVVF;
⊃ RCUBES[.,12]-Y comp of the normal to SVVF;
⊃ ICUBES[.,1]-pointer to cube on top (if any);
⊃ ICUBES[.,2]-pointer to supporting cube (if any), -1 if not supported;
⊃ ICUBES[.,3]-1 if cube is grasped, 0 otherwise;
⊃ ICUBES[.,4]-2 if active (c.t.c known), 1 if inactive (only h.t.c. known),
0 if disappeared;
⊃ ICUBES[.,5]-1 if cube is unstable, 0 otherwise;
⊃ RTABLE[.,.,1]-correction needed in x-direction of i.c.s;
⊃ RTABLE[.,.,2]-correction needed in y-direction of i.c.s;
⊃ ITABL[.,.]- 1 if correction found, 0 otherwise;
⊃ RHAND[1]-grasping error in x-direction of h.c.s;
⊃ RHAND[2]-grasping error in z-direction of h.c.s;
⊃ HCUBE-pointer to cube grasped;
REQUIRE "HANDSER.SAI[SYS,HE]" SOURCE_FILE;
SIMPLE PROCEDURE UNGRASP(REAL WID);
BEGIN "UNGRASP"
IF (HCUBE≠0)∧(ICUBES[HCUBE,5]≠0) THEN BEGIN
OUTSTR("UNGRASP-FAILED: CUBE NO. "&CVS(HCUBE)&" IS UNSTABLE"CRLF);
STKFLG←31; RETURN; END;
IF HCUBE≠0 THEN ICUBES[HCUBE,3]←0;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_OPEN(WID));
IF ARM_STATUS≠0 THEN BEGIN
OUTSTR("UNGRASP-FAILED: NO UNGRASP (ARM_STATUS="&CVOS(ARM_STATUS)&
")"CRLF); STKFLG←4; RETURN; END;
IF HCUBE≠0 THEN BEGIN
RCUBES[HCUBE,8]←ARM_LINK[6,1,4];
RCUBES[HCUBE,9]←ARM_LINK[6,2,4];
RCUBES[HCUBE,10]←ARM_LINK[6,3,4]; END;
HCUBE←0;
END "UNGRASP";
PROCEDURE FIND_HAND(REFERENCE REAL XIR,YIR,XIL,YIL,XER,YER; BOOLEAN CENT);
BEGIN "FIND_HAND"
REAL ARRAY DIRD[1:8];
REAL XTC,YTC,ZTC;
INTEGER I,J;
BOOLEAN TRIED1,TRIED2;
LABEL TRY1,TRY2;
ARMTRAN(0,-((GRASP-GCOR)/2+THICK),MARKZ,XTC,YTC,ZTC);
IF CENT THEN BEGIN
MESS←ISSUE(7,"DRV","CAM",MESSAGE CAM_CENTER(CAMLENS,XTC,YTC,ZTC));
IF CAMFLG≠0 THEN BEGIN STKFLG←7; RETURN; END; END;
TRIED1←TRIED2←FALSE; LOOK_AT[4]←LOOK_AT[5]←18;
TRY1: CORN_HAND(MARKX,-((GRASP-GCOR)/2+THICK),MARKZ,DIRD);
LOOK_AT[2]←DIRD[5]+XER; LOOK_AT[3]←DIRD[6]+YER;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(1,1,2,.05,DIRD));
IF EYEFLG≠0 THEN IF TRIED2 THEN BEGIN STKFLG←8; RETURN; END ELSE GOTO TRY2;
XIR←DIR_EYE[1,5]; YIR←DIR_EYE[1,6]; TRIED1←TRUE;
IF ¬TRIED2 THEN BEGIN
XER←DIR_EYE[1,5]-DIRD[5]; YER←DIR_EYE[1,6]-DIRD[6];
TRY2: CORN_HAND(-MARKX,-((GRASP-GCOR)/2+THICK),MARKZ,DIRD);
LOOK_AT[2]←DIRD[5]+XER; LOOK_AT[3]←DIRD[6]+YER;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(1,1,2,.05,DIRD));
IF EYEFLG≠0 THEN BEGIN STKFLG←8; RETURN; END;
XIL←DIR_EYE[1,5]; YIL←DIR_EYE[1,6]; TRIED2←TRUE;
IF ¬TRIED1 THEN BEGIN
XER←DIR_EYE[1,5]-DIRD[5]; YER←DIR_EYE[1,6]-DIRD[6]; GOTO TRY1; END;
END;
IF (ABS(DIR_EYE[1,5]-DIRD[5]-XER)>3)∧(ABS(DIR_EYE[1,6]-DIRD[6]-YER)>3)
THEN BEGIN OUTSTR("PLAS_CUBE-FAILED: HAND-MARK NOT FOUND"CRLF);
STKFLG←8; RETURN; END;
I←(ARM_LINK[6,1,4]+1.9)/4; J←(ARM_LINK[6,2,4]+3.9)/4;
RTABL[I,J,1]←XER; RTABL[I,J,2]←YER; ITABL[I,J]←1;
OUTSTR("FIND_HAND: CAMCOR=("&CVG(XER)&CVG(YER)&")"CRLF);
END "FIND_HAND";
SIMPLE PROCEDURE ADAPT_ARM(REAL XH,YH,ZH;REFERENCE REAL XER,YER);
BEGIN "ADAPT_ARM"
REAL DIRX,DIRY,DUM;
INTEGER I,J;
FORTRAN REAL PROCEDURE SQRT(REAL X);
IF (XH<2.15)∨(XH>42.05)∨(YH<2)∨(YH>40.05) THEN BEGIN
OUTSTR("ADAPT_ARM-FAILED: OUTSIDE OF WORK AREA"CRLF);
STKFLG←7; RETURN; END;
I←(XH+1.9)/4; J←(YH+3.9)/4;
IF ITABL[I,J]=1 THEN BEGIN
XER←RTABL[I,J,1]; YER←RTABL[I,J,2]; RETURN; END;
UNGRASP(3); IF STKFLG≠0 THEN RETURN;
DIRY←SQRT((XH-P0X)↑2+(YH-P0Y)↑2);
DIRX←(XH-P0X)/DIRY; DIRY←(YH-P0Y)/DIRY;
POSE_HAND(XH,YH,ZH,DIRX,DIRY,STKFLG);
IF STKFLG≠0 THEN RETURN; WAIT;
FIND_HAND(DUM,DUM,DUM,DUM,XER,YER,TRUE);
END "ADAPT_ARM";
SIMPLE PROCEDURE PLACE(INTEGER BOT);
BEGIN "PLACE"
IF BOT≠0 THEN ICUBES[BOT,1]←HCUBE;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_PLACE);
IF ARM_STATUS≠0
THEN BEGIN OUTSTR("PLACE-FAILED: NO PLACE (ARM_STATUS="
&CVOS(ARM_STATUS)&")"CRLF);
STKFLG←5; RETURN; END;
ICUBES[HCUBE,2]←BOT;
IF BOT=0 THEN ICUBES[HCUBE,5]←0; WAIT;
END "PLACE";
SIMPLE PROCEDURE LNORML(REAL XIB,YIB,XID,YID; REAL ARRAY DIRE);
BEGIN "LNORML"
REAL D;
FORTRAN REAL PROCEDURE SQRT(REAL X);
D←SQRT((XIB-XID)↑2+(YIB-YID)↑2);
DIRE[1]←(XIB-XID)/D; DIRE[2]←(YIB-YID)/D;
IF DIRE[1]=0
THEN DIRE[2]←-ABS(DIRE[2])
ELSE IF DIRE[1]>0
THEN BEGIN DIRE[1]←-DIRE[1]; DIRE[2]←-DIRE[2]; END;
DIRE[3]←-DIRE[1]; DIRE[4]←-DIRE[2];
DIRE[5]←(XIB+XID)/2; DIRE[6]←(YIB+YID)/2; DIRE[7]←1; DIRE[8]←0;
END "LNORML";
PROCEDURE FIND_CUBE(INTEGER CUB,BKGR,INT; REAL XTC,YTC);
BEGIN "FIND_CUBE"
INTEGER CORNUM,J;
REAL ARRAY DIRD[1:8],XT,YT,XI,YI[1:3];
REAL D1,D2,M;
LABEL EBU;
BOOLEAN AGAIN;
FORTRAN REAL PROCEDURE SQRT(REAL X);
IF (CUB<1)∨(CUB>8) THEN BEGIN
OUTSTR("FIND_CUBE-FAILED: ILLEGEAL CUBE NO."CRLF); RETURN; END;
IF (ICUBES[CUB,4]≠0)∧(ICUBES[CUB,2]≠0) THEN BEGIN
OUTSTR("FIND_CUBE-FAILED: CUBE NOT ON TABLE"CRLF); RETURN; END;
FOR J←1 STEP 1 UNTIL 8 DO
IF (J≠CUB)∧(ICUBES[J,4]≠0)∧
SQRT((XTC-RCUBES[J,1])↑2+(YTC-RCUBES[J,2])↑2)<3*RCUBES[J,6]/2 THEN BEGIN
OUTSTR("FIND_CUBE-FAILED: TOO CLOSE TO CUBE NO. "&CVS(J)CRLF);
RETURN; END;
MESS←ISSUE(7,"DRV","CAM",MESSAGE CAM_CENTER(CAMLENS,XTC,YTC,0));
IF CAMFLG≠0 THEN RETURN;
CORNUM←0; J←CUB;
LOOK_AT[1]←1; LOOK_AT[4]←LOOK_AT[5]←18;
OUTSTR("...START FROM LEFT BOTTOM CORNER..."CRLF);
EBU: OUTSTR("...SETWINDOW ON CORNER NO. "&CVS(CORNUM+1)&"..."CRLF);
MESS←ISSUE(7,"DRV","EYE",MESSAGE SET_WINDOW);
IF EYEFLG≠0 THEN GOTO EBU;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(BKGR,INT,0,-1,DIRD));
IF (EYEFLG=0)∧(DIR_EYE[1,7]=0)
THEN BEGIN CORNUM←CORNUM+1;
XI[CORNUM]←DIR_EYE[1,5]; YI[CORNUM]←DIR_EYE[1,6];
TABLECOR(DIR_EYE[1,5],DIR_EYE[1,6],XT[CORNUM],YT[CORNUM]);
END
ELSE IF CORNUM<2 THEN BEGIN
OUTSTR(",,,TYPE Y TO LOOK AGAIN FOR CORNER:"CRLF);
IF YES THEN GOTO EBU ELSE RETURN; END;
IF CORNUM=2
THEN BEGIN OUTSTR("...TYPE Y IF FOR 3RD. CORNER:"CRLF);
IF YES
THEN GOTO EBU
ELSE BEGIN OUTSTR("FIND_CUBE: DEGENERATE VIEW"CRLF);
RCUBES[J,6]←SQRT((XT[1]-XT[2])↑2+(YT[1]-YT[2])↑2);
RCUBES[J,1]←((XT[1]+XT[2])-(YT[2]-YT[1]))/2;
RCUBES[J,2]←((YT[1]+YT[2])-(XT[1]-XT[2]))/2;
RCUBES[J,4]←(YT[2]-YT[1])/RCUBES[J,6];
RCUBES[J,5]←(XT[1]-XT[2])/RCUBES[J,6];
END;
END
ELSE IF CORNUM=3
THEN BEGIN
IF ABS((D1←SQRT((XT[1]-XT[2])↑2+(YT[1]-YT[2])↑2))-
(D2←SQRT((XT[3]-XT[2])↑2+(YT[3]-YT[2])↑2)))<.2
THEN BEGIN RCUBES[J,6]←(D1+D2)/2;
RCUBES[J,1]←(XT[1]+XT[3])/2;
RCUBES[J,2]←(YT[1]+YT[3])/2;
IF SQRT((XI[2]-XI[3])↑2+(YI[2]-YI[3])↑2)>
SQRT((XI[2]-XI[1])↑2+(YI[2]-YI[1])↑2)
THEN BEGIN RCUBES[J,4]←(XT[2]-XT[3])/D2;
RCUBES[J,5]←(YT[2]-YT[3])/D2; END
ELSE BEGIN RCUBES[J,4]←(XT[2]-XT[1])/D1;
RCUBES[J,5]←(YT[2]-YT[1])/D1; END;
END
ELSE BEGIN OUTSTR("FIND_CUBE: SIDES UNEQUAL (D1="&CVG(D1)
&" D2="&CVG(D2)&")"CRLF); RETURN; END;
END
ELSE GOTO EBU;
RCUBES[J,3]←RCUBES[J,6]/2;
RCUBES[J,8]←RCUBES[J,1]; RCUBES[J,9]←RCUBES[J,2];
RCUBES[J,10]←3*RCUBES[J,6]/4;
M←((P0X-RCUBES[J,1])*RCUBES[J,5]-(P0Y-RCUBES[J,2])*RCUBES[J,4])/
SQRT((P0X-RCUBES[J,1])↑2+(P0Y-RCUBES[J,2])↑2);
IF M>.707
THEN BEGIN RCUBES[J,4]↔RCUBES[J,5]; RCUBES[J,5]←-RCUBES[J,5];
RCUBES[J,7]←-SQRT(1-M↑2); END
ELSE IF M<-.707
THEN BEGIN RCUBES[J,4]↔RCUBES[J,5]; RCUBES[J,4]←-RCUBES[J,4];
RCUBES[J,7]←SQRT(1-M↑2); END
ELSE RCUBES[J,7]←M;
IF RCUBES[J,7]<0
THEN BEGIN RCUBES[J,11]←-RCUBES[J,5]; RCUBES[J,12]←RCUBES[J,4]; END
ELSE BEGIN RCUBES[J,11]←RCUBES[J,5]; RCUBES[J,12]←-RCUBES[J,4]; END;
ICUBES[J,1]←ICUBES[J,2]←0; ICUBES[J,4]←2; ICUBES[J,5]←0;
END "FIND_CUBE";
PROCEDURE GRSP_CUBE(INTEGER CUB);
BEGIN "GRSP_CUBE"
INTEGER FINGER,I,J;
REAL XTC,YTC,ZTC,XT1,XT2,YT1,YT2,ZT1,ZT2,GAP,
XCC,YCC,ZCC,A,B,C,DUM,XER,YER;
REAL ARRAY INC[1:4];
LABEL KHC;
FORTRAN REAL PROCEDURE SQRT(REAL X);
IF ICUBES[CUB,3]=1 THEN RETURN;
IF ICUBES[CUB,4]≠2 THEN BEGIN
OUTSTR("GRSP_CUBE-FAILED: CUBE IS INACTIVE"CRLF);
STKFLG←5; RETURN; END;
IF ICUBES[CUB,1]≠0 THEN BEGIN
OUTSTR("GRSP_CUBE-FAILED: TOP IS OCCUPIED"CRLF);
STKFLG←5; RETURN; END;
ADAPT_ARM(RCUBES[CUB,1],RCUBES[CUB,2],RCUBES[CUB,3]+2*RCUBES[CUB,6],
XER,YER);
IF STKFLG≠0 THEN RETURN;
UNGRASP(3); IF STKFLG≠0 THEN RETURN;
POSE_HAND(RCUBES[CUB,8],RCUBES[CUB,9],RCUBES[CUB,10],
-RCUBES[CUB,4],-RCUBES[CUB,5],STKFLG);
IF STKFLG≠0 THEN RETURN; WAIT;
KHC: S_O_T←TRUE;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_OPEN(RCUBES[CUB,6]/2));
IF (ARM_STATUS≠6)∧(ARM_STATUS≠'16) THEN BEGIN
OUTSTR("GRSP_CUBE-FAILED: NO TOUCH"CRLF); STKFLG←5; RETURN; END;
S_O_T←FALSE;
IF FELT[1,1,1]>0
THEN FINGER←1
ELSE IF FELT[2,1,1]>0
THEN FINGER←2
ELSE BEGIN OUTSTR("GRSP_CUBE-FAILED: S_O_T BUT NO TOUCH"CRLF);
STKFLG←6; RETURN; END;
OUTSTR("GRSP_CUBE: FINGER NO. "&CVS(FINGER)&" TOUCHED"CRLF);
IF FINGER=2
THEN GAP←THICK+RCUBES[CUB,6]/2
ELSE GAP←THICK+GRASP-GCOR-RCUBES[CUB,6]/2;
IF DBUG THEN OUTSTR("GRSP_CUBE: GAP="&CVG(GAP)&
" GRASP="&CVG(GRASP-GCOR)CRLF);
WAIT;
FIND_HAND(XT1,YT1,XT2,YT2,XER,YER,TRUE);
IF STKFLG≠0 THEN RETURN;
TABLECOR(XT1,YT1,XT1,YT1); TABLECOR(XT2,YT2,XT2,YT2);
XCC←CAMERA_MODEL[4,1]; YCC←CAMERA_MODEL[4,2]; ZCC←CAMERA_MODEL[4,3];
A←RCUBES[CUB,4]; B←RCUBES[CUB,5];
C←-(GAP+A*RCUBES[CUB,1]+B*RCUBES[CUB,2]);
DUM←-(C+A*XT1+B*YT1)/(B*(YCC-YT1)+A*(XCC-XT1));
YT1←YT1+(YCC-YT1)*DUM; XT1←XT1+(XCC-XT1)*DUM; ZT1←ZCC*DUM;
DUM←-(C+A*XT2+B*YT2)/(B*(YCC-YT2)+A*(XCC-XT2));
YT2←YT2+(YCC-YT2)*DUM; XT2←XT2+(XCC-XT2)*DUM; ZT2←ZCC*DUM;
IF DBUG THEN OUTSTR("GRSP_CUBE: R. COR=("&CVG(XT1)&CVG(YT1)&CVG(ZT1)&")"
CRLF&" L. COR=("&CVG(XT2)&CVG(YT2)&CVG(ZT2)&")"
CRLF);
IF ABS(ZT1-ZT2)>.1 THEN BEGIN
OUTSTR("GRSP_CUBE-FAILED: INEQUAL HIGHT ("&CVG(ZT1-ZT2)&")"CRLF);
STKFLG←9; RETURN; END;
IF (DUM←ABS(SQRT((XT1-XT2)↑2+(YT1-YT2)↑2)-.6))>.1 THEN BEGIN
OUTSTR("GRSP_CUBE-FAILED: ILLEGAL WIDTH ("&CVG(DUM)&")"CRLF);
STKFLG←9; RETURN; END;
XTC←(XT1+XT2)/2-(THICK+(GRASP-GCOR)/2)*A;
YTC←(YT1+YT2)/2-(THICK+(GRASP-GCOR)/2)*B;
ZTC←(ZT1+ZT2)/2;
INC[1]←RCUBES[CUB,1]-XTC; INC[2]←RCUBES[CUB,2]-YTC;
INC[3]←RCUBES[CUB,3]+RCUBES[CUB,6]/4-ZTC; INC[4]←1;
OUTSTR("GRSP_CUBE: INC=("&CVG(INC[1])&CVG(INC[2])&CVG(INC[3])&")"CRLF);
RHAND[1]←INC[2]*A-INC[1]*B; RHAND[2]←INC[3];
IF DBUG THEN
OUTSTR("GRSP_HAND: RHAND=("&CVG(RHAND[1])&CVG(RHAND[2])&")"CRLF);
WAIT;
IF (ABS(INC[1])<.1)∧(ABS(INC[2])<.1)∧(ABS(INC[3])<.1) THEN BEGIN
HCUBE←CUB;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_CLOSE(RCUBES[CUB,6]/2));
IF ARM_STATUS≠0
THEN BEGIN OUTSTR("GRIP-FAILED: NO GRIP (ARM_STATUS="&
CVOS(ARM_STATUS)&")"CRLF); STKFLG←4; END
ELSE ICUBES[CUB,3]←1; WAIT;
RETURN; END;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_OPEN(GRASP-GCOR+.2));
IF ARM_STATUS≠0 THEN BEGIN
OUTSTR("GRSP_CUBE-FAILED: NO OPEN (ARM_STATUS="&CVOS(ARM_STATUS)&
")"CRLF); STKFLG←4; RETURN; END; WAIT;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_CHANGE(INC,.1,ARM_PLAN));
IF (ARM_STATUS≠0)∨(¬ARM_PLAN) THEN BEGIN
OUTSTR("GRSP_CUBE-FAILED: NO CORRECTION (ARM_STATUS="
&CVOS(ARM_STATUS)&")"CRLF); STKFLG←12; RETURN; END; WAIT;
GOTO KHC;
END "GRSP_CUBE";
PROCEDURE STAK_CUBE(INTEGER BOT,TOP; REAL DELM,DELS);
BEGIN"STAK_CUBE"
REAL XT1,YT1,XT2,YT2,XT3,YT3,ZT,XIB,YIB,XIC,YIC,XID,YID,DUM,XTC,YTC,
WID,HIT,ZTH,XER,YER,TCENX,TCENY,TCENZ,XB1,XB2,XB3,YB1,YB2,YB3;
REAL ARRAY DIRD1,DIRD2,DIRD3,DIRD4[1:8],INC[1:4],DIS,IND[1:5];
INTEGER K,KMAX;
LABEL KHC;
IF (BOT>8)∨(BOT<1)∨(ICUBES[BOT,4]≠2) THEN BEGIN
OUTSTR("STAK_CUBE-FAILED: BOTTOM CUBE DOES NOT EXIST OR INACTIVE("&
CVS(BOT)&")"CRLF);
RETURN END;
IF (TOP>8)∨(TOP<1)∨(ICUBES[TOP,4]=0) THEN BEGIN
OUTSTR("STAK_CUBE-FAILED: TOP CUBE DOES NOT EXIST ("&CVS(TOP)&")"CRLF);
RETURN; END;
IF TOP=BOT THEN BEGIN
OUTSTR("STAK_CUBE-FAILED: CANNOT STACK A CUBE ON ITSELF"CRLF);
RETURN; END;
IF ICUBES[BOT,1]≠0 THEN BEGIN
OUTSTR("STAK_CUBE-FAILED: TOP OF BOTTOM CUBE IS OCCUPIED"CRLF);
RETURN; END;
ADAPT_ARM(RCUBES[BOT,1],RCUBES[BOT,2],RCUBES[BOT,3]+2*RCUBES[BOT,6],
XER,YER);
IF STKFLG≠0 THEN RETURN;
GRSP_CUBE(TOP);
IF STKFLG≠0 THEN RETURN;
ZT←RCUBES[BOT,3]+RCUBES[BOT,6]/2;
XB1←RCUBES[BOT,1]+RCUBES[BOT,6]*(RCUBES[BOT,4]+RCUBES[BOT,11])/2;
YB1←RCUBES[BOT,2]+RCUBES[BOT,6]*(RCUBES[BOT,5]+RCUBES[BOT,12])/2;
XB2←RCUBES[BOT,1]+RCUBES[BOT,6]*(RCUBES[BOT,4]-RCUBES[BOT,11])/2;
YB2←RCUBES[BOT,2]+RCUBES[BOT,6]*(RCUBES[BOT,5]-RCUBES[BOT,12])/2;
XB3←2*RCUBES[BOT,1]-XB2; YB3←2*RCUBES[BOT,2]-YB2;
MESS←ISSUE(7,"DRV","CAM",MESSAGE CAM_CENTER(CAMLENS,XB1/2+(XB2+XB3)/4,
YB1/2+(YB2+YB3)/4,ZT));
IF CAMFLG≠0 THEN BEGIN STKFLG←4; RETURN; END;
IMAGECOR(XB1,YB1,ZT,XIC,YIC); IMAGECOR(XB2,YB2,ZT,XIB,YIB);
IMAGECOR(XB3,YB3,ZT,XID,YID);
LNORML(XIB,YIB,XIC,YIC,DIRD1); LNORML(XIC,YIC,XID,YID,DIRD2);
WAIT;
IF ABS(DIRD1[1])<.707
THEN BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END
ELSE BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END;
LOOK_AT[2]←DIRD1[5]; LOOK_AT[3]←DIRD1[6]; DIRD1[8]←3;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(0,0,-1,.05,DIRD1));
IF DIR_EYE[0,1]<2 THEN BEGIN STKFLG←8; RETURN; END; WAIT;
ARRBLT(DIRD1[1],DIR_EYE[1,1],8);
IF DIRD1[7]=0 THEN LNORML(DIRD1[5],DIRD1[6],DIRD1[5]+DIRD1[2*DIRD1[8]-1],
DIRD1[6]+DIRD1[2*DIRD1[8]],DIRD1);
IF ABS(DIRD2[1])<.707
THEN BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END
ELSE BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END;
LOOK_AT[2]←DIRD2[5]; LOOK_AT[3]←DIRD2[6]; DIRD2[8]←3;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(0,0,-1,.05,DIRD2));
IF DIR_EYE[0,1]<2 THEN BEGIN STKFLG←8; RETURN; END; WAIT;
ARRBLT(DIRD2[1],DIR_EYE[1,1],8);
IF DIRD2[7]=0 THEN LNORML(DIRD2[5],DIRD2[6],DIRD2[5]+DIRD2[2*DIRD2[8]-1],
DIRD2[6]+DIRD2[2*DIRD2[8]],DIRD2);
DUM←DIRD1[1]*DIRD2[2]-DIRD1[2]*DIRD2[1];
XIC←(DIRD1[1]*DIRD2[1]*(DIRD1[6]-DIRD2[6])+DIRD1[1]*DIRD2[2]*DIRD2[5]-
DIRD1[2]*DIRD2[1]*DIRD1[5])/DUM;
YIC←(DIRD1[2]*DIRD2[2]*(DIRD2[5]-DIRD1[5])+DIRD1[1]*DIRD2[2]*DIRD1[6]-
DIRD1[2]*DIRD2[1]*DIRD2[6])/DUM;
TABLECOR(XIC,YIC,XTC,YTC);
DUM←ZT/CAMERA_MODEL[4,3];
XTC←XTC+(CAMERA_MODEL[4,1]-XTC)*DUM;
YTC←YTC+(CAMERA_MODEL[4,2]-YTC)*DUM;
OUTSTR("STAK_CUBE: BOT-CORNER=("&CVG(XTC)&CVG(YTC)&")"CRLF);
XTC←XTC-DELM*RCUBES[BOT,4]-DELS*RCUBES[BOT,11];
YTC←YTC-DELM*RCUBES[BOT,5]-DELS*RCUBES[BOT,12];
OUTSTR("STAK_CUBE: REQ. POS. OF TOP-CORNER=("&CVG(XTC)&CVG(YTC)&")"CRLF);
TCENX←XTC-(RCUBES[BOT,4]+RCUBES[BOT,11])*RCUBES[TOP,6]/2;
TCENY←YTC-(RCUBES[BOT,5]+RCUBES[BOT,12])*RCUBES[TOP,6]/2;
TCENZ←ZT+RCUBES[BOT,6]/2;
POSE_HAND(RCUBES[BOT,8]-RCUBES[BOT,1]+TCENX,
RCUBES[BOT,9]-RCUBES[BOT,2]+TCENY,
1.0+TCENZ+RCUBES[TOP,6]/4,
-RCUBES[BOT,4],-RCUBES[BOT,5],STKFLG);
IF STKFLG=1 THEN RETURN;
ICUBES[TOP,4]←1; ICUBES[TOP,5]←1;
IF STKFLG=2 THEN BEGIN ICUBES[TOP,2]←-1; RETURN; END;
IF (K←ICUBES[TOP,2])>0 THEN ICUBES[K,1]←0; ICUBES[TOP,2]←-1; WAIT;
PLACE(BOT); IF STKFLG≠0 THEN RETURN;
FOR K←4,5,7,11,12 DO RCUBES[TOP,K]←RCUBES[BOT,K];
RCUBES[TOP,1]←TCENX; RCUBES[TOP,2]←TCENY; RCUBES[TOP,3]←TCENZ;
FIND_HAND(DUM,DUM,DUM,DUM,XER,YER,FALSE);
IF STKFLG≠0 THEN RETURN;
KHC: WID←RCUBES[TOP,6]/2; HIT←3*WID/2-RHAND[2];
ARMTRAN(WID-RHAND[1],-WID,HIT,XT1,YT1,ZTH);
IMAGECOR(XT1,YT1,ZTH,XIC,YIC);
ARMTRAN(-WID-RHAND[1],-WID,HIT,XT2,YT2,ZTH);
IMAGECOR(XT2,YT2,ZTH,XIB,YIB);
IF RCUBES[TOP,7]≤0
THEN BEGIN ARMTRAN(-WID-RHAND[1],WID,HIT,XT3,YT3,ZTH);
XIB↔XIC; YIB↔YIC; END
ELSE ARMTRAN(WID-RHAND[1],WID,HIT,XT3,YT3,ZTH);
IMAGECOR(XT3,YT3,ZTH,XID,YID);
IF DBUG THEN OUTSTR(CVG(XT2)&CVG(YT2)&CVG(XT1)&CVG(YT1)&
CVG(XT3)&CVG(YT3)CRLF); WAIT;
LNORML(XIB,YIB,XIC,YIC,DIRD3); LNORML(XIC,YIC,XID,YID,DIRD4);
IF ABS(DIRD1[1])<.707
THEN BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END
ELSE BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END;
LOOK_AT[2]←DIRD3[5]+XER; LOOK_AT[3]←DIRD3[6]+YER;
DIRD1[8]←3;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(0,0,-9,.05,DIRD1));
IF DIR_EYE[0,1]<2 THEN
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(-1,0,-9,.05,DIRD1));
IF ((KMAX←(DIR_EYE[0,1]-1))<1)∨(KMAX>5)
THEN BEGIN OUTSTR("STAK_CUBE-FAILED: 1ST. EDGE DISAPPEARED"CRLF);
STKFLG←8; RETURN; END;
IF KMAX=1
THEN ARRBLT(DIRD3[1],DIR_EYE[1,1],8)
ELSE BEGIN INTEGER ORD; ORD←0;
FOR K←1 STEP 1 UNTIL KMAX DO BEGIN
DIS[K]←(DIRD1[5]-DIR_EYE[K,5])*DIRD1[2]
-(DIRD1[6]-DIR_EYE[K,6])*DIRD1[1];
IND[K]←K; END;
WHILE ORD=0 DO BEGIN ORD←1;
FOR K←2 STEP 1 UNTIL KMAX DO
IF DIS[K]<DIS[K-1] THEN BEGIN
DIS[K]↔DIS[K-1]; IND[K]↔IND[K-1]; ORD←0; END;
END;
IF (DIS[1]<-1)∨(DIS[1]>1)
THEN ARRBLT(DIRD3[1],DIR_EYE[IND[1],1],8)
ELSE ARRBLT(DIRD3[1],DIR_EYE[IND[2],1],8);
END;
FOR K←1 STEP 1 UNTIL 6 DO OUTSTR(CVG(DIRD3[K])); OUTSTR(""CRLF); WAIT;
IF DIRD3[7]=0 THEN LNORML(DIRD3[5],DIRD3[6],DIRD3[5]+DIRD3[2*DIRD3[8]-1],
DIRD3[6]+DIRD3[2*DIRD3[8]],DIRD3);
IF ABS(DIRD2[1])<.707
THEN BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END
ELSE BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END;
LOOK_AT[2]←DIRD4[5]+XER; LOOK_AT[3]←DIRD4[6]+YER;
DIRD2[8]←3;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(0,0,-9,.05,DIRD2));
IF DIR_EYE[0,1]<2 THEN
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(-1,0,-9,.05,DIRD2));
IF ((KMAX←(DIR_EYE[0,1]-1))<1)∨(KMAX>5)
THEN BEGIN OUTSTR("STAK_CUBE-FAILED: 2ND. EDGE DISAPPEARED"CRLF);
STKFLG←8; RETURN; END;
IF KMAX=1
THEN ARRBLT(DIRD4[1],DIR_EYE[1,1],8)
ELSE BEGIN INTEGER ORD; ORD←0;
FOR K←1 STEP 1 UNTIL KMAX DO BEGIN
DIS[K]←(DIRD2[5]-DIR_EYE[K,5])*DIRD2[2]
-(DIRD2[6]-DIR_EYE[K,6])*DIRD2[1];
IND[K]←K; END;
WHILE ORD=0 DO BEGIN ORD←1;
FOR K←2 STEP 1 UNTIL KMAX DO
IF DIS[K]<DIS[K-1] THEN BEGIN
DIS[K]↔DIS[K-1]; IND[K]↔IND[K-1]; ORD←0; END;
END;
IF (DIS[1]<-1)∨(DIS[1]>1)
THEN ARRBLT(DIRD4[1],DIR_EYE[IND[1],1],8)
ELSE ARRBLT(DIRD4[1],DIR_EYE[IND[2],1],8);
END;
FOR K←1 STEP 1 UNTIL 6 DO OUTSTR(CVG(DIRD4[K])); OUTSTR(""CRLF); WAIT;
IF DIRD4[7]=0 THEN LNORML(DIRD4[5],DIRD4[6],DIRD4[5]+DIRD4[2*DIRD4[8]-1],
DIRD4[6]+DIRD4[2*DIRD4[8]],DIRD4);
DUM←DIRD3[1]*DIRD4[2]-DIRD3[2]*DIRD4[1];
XIC←(DIRD3[1]*DIRD4[1]*(DIRD3[6]-DIRD4[6])+DIRD3[1]*DIRD4[2]*DIRD4[5]-
DIRD3[2]*DIRD4[1]*DIRD3[5])/DUM;
YIC←(DIRD3[2]*DIRD4[2]*(DIRD4[5]-DIRD3[5])+DIRD3[1]*DIRD4[2]*DIRD3[6]-
DIRD3[2]*DIRD4[1]*DIRD4[6])/DUM;
TABLECOR(XIC,YIC,XT1,YT1);
DUM←ZT/CAMERA_MODEL[4,3];
XT1←XT1+(CAMERA_MODEL[4,1]-XT1)*DUM;
YT1←YT1+(CAMERA_MODEL[4,2]-YT1)*DUM;
XT2←XT1-RCUBES[TOP,6]*(RCUBES[BOT,4]+RCUBES[BOT,11])/2;
YT2←YT1-RCUBES[TOP,6]*(RCUBES[BOT,5]+RCUBES[BOT,12])/2;
RCUBES[TOP,1]←XT2; RCUBES[TOP,2]←YT2; ICUBES[TOP,4]←2;
OUTSTR("STAK_CUBE: TOP-CORNER=("&CVG(XT1)&CVG(YT1)&")"&
" ERROR=("&CVG(XTC-XT1)&CVG(YTC-YT1)&")"CRLF);
IF (ABS(XTC-XT1)<.1)∧(ABS(YTC-YT1)<.1) THEN BEGIN
IF (ABS((XT2-RCUBES[BOT,1])*RCUBES[BOT,4]+
(YT2-RCUBES[BOT,2])*RCUBES[BOT,5])<(DUM←(RCUBES[BOT,6]/2-.2)))∧
(ABS((XT2-RCUBES[BOT,1])*RCUBES[BOT,5]-
(YT2-RCUBES[BOT,2])*RCUBES[BOT,4])<(DUM)) THEN ICUBES[TOP,5]←0;
RETURN; END; WAIT;
IF DBUG THEN OUTSTR("HAND AT:"&CVG(ARM_LINK[6,1,4])&CVG(ARM_LINK[6,2,4])CRLF);
INC[1]←INC[2]←0; INC[3]←1.0; INC[4]←1;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_CHANGE(INC,.5,ARM_PLAN));
IF ¬ARM_PLAN THEN BEGIN
OUTSTR("STAK_CUBE-FAILED: NO PLAN TO LIFT"CRLF);
STKFLG←5; RETURN; END;
ICUBES[TOP,2]←-1; ICUBES[TOP,4]←1; ICUBES[TOP,5]←1;
IF ARM_STATUS≠0 THEN BEGIN
OUTSTR("STAK_CUBE-FAILED: NO LIFT (ARM_STATUS="
&CVOS(ARM_STATUS)&")"CRLF);
STKFLG←5; RETURN; END;
ICUBES[BOT,1]←0; WAIT;
INC[1]←XTC-XT1; INC[2]←YTC-YT1; INC[3]←0;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_CHANGE(INC,.1,ARM_PLAN));
IF (ARM_STATUS≠0)∨(¬ARM_PLAN) THEN BEGIN
OUTSTR("STAK_CUBE-FAILED: NO CORRECTION (ARM_STATUS="
&CVOS(ARM_STATUS)&")"CRLF);
STKFLG←5; RETURN; END; WAIT;
PLACE(BOT); IF STKFLG≠0 THEN RETURN;
IF DBUG THEN OUTSTR("HAND AT:"&CVG(ARM_LINK[6,1,4])&CVG(ARM_LINK[6,2,4])CRLF);
GOTO KHC;
END "STAK_CUBE";
PROCEDURE PLAS_CUBE(INTEGER CUB;REAL XTC,YTC);
BEGIN "PLAS_CUBE"
REAL XT1,YT1,XT2,YT2,XT3,YT3,ZTH,XIB,YIB,XIC,YIC,XID,YID,DUM,M,DIRX,DIRY,
XER,YER,WID,HIT;
REAL ARRAY DIRD1,DIRD2,DIRD3,DIRD4[1:8],INC[1:4],DIS,IND[1:5];
INTEGER K,KMAX;
LABEL KHC;
FORTRAN REAL PROCEDURE SQRT(REAL X);
IF (CUB>8)∨(CUB<1)∨(ICUBES[CUB,4]=0) THEN BEGIN
OUTSTR("PLAS_CUBE-FAILED: CUBE NO. "&CVS(CUB)&" DOES NOT EXIST"CRLF);
RETURN; END;
FOR K←1 STEP 1 UNTIL 8 DO
IF (K≠CUB)∧(ICUBES[K,4]≠0)∧
(SQRT((XTC-RCUBES[K,1])↑2+(YTC-RCUBES[K,2])↑2)<RCUBES[K,6])
THEN BEGIN OUTSTR("PLAS_CUBE-FAILED: TOO CLOSE TO CUBE NO. "&CVS(K)CRLF);
RETURN; END;
ADAPT_ARM(XTC,YTC,RCUBES[CUB,6],XER,YER);
IF STKFLG≠0 THEN RETURN;
GRSP_CUBE(CUB);
IF STKFLG≠0 THEN RETURN;
M←((P0X-XTC)*RCUBES[CUB,5]-(P0Y-YTC)*RCUBES[CUB,4])/
SQRT((P0X-XTC)↑2+(P0Y-YTC)↑2);
IF M >.707
THEN BEGIN DIRX←-RCUBES[CUB,5]; DIRY←RCUBES[CUB,4]; END
ELSE IF M<-.707
THEN BEGIN DIRX←RCUBES[CUB,5]; DIRY←-RCUBES[CUB,4]; END
ELSE BEGIN DIRX←-RCUBES[CUB,4]; DIRY←-RCUBES[CUB,5]; END;
POSE_HAND(XTC,YTC,3*RCUBES[CUB,4]/4+1.0,DIRX,DIRY,STKFLG);
IF STKFLG=1 THEN RETURN;
ICUBES[CUB,4]←1; ICUBES[CUB,5]←1;
IF STKFLG=2 THEN BEGIN ICUBES[CUB,2]←-1; RETURN; END;
IF (K←ICUBES[CUB,2])>0 THEN ICUBES[K,1]←0; ICUBES[CUB,2]←-1; WAIT;
PLACE(0); IF STKFLG≠0 THEN RETURN;
RCUBES[CUB,1]←XTC; RCUBES[CUB,2]←YTC; RCUBES[CUB,3]←RCUBES[CUB,6]/2;
RCUBES[CUB,4]←-DIRX; RCUBES[CUB,5]←-DIRY;
IF M>.707
THEN RCUBES[CUB,7]←-SQRT(1-M↑2)
ELSE IF M<-.707
THEN RCUBES[CUB,7]←SQRT(1-M↑2)
ELSE RCUBES[CUB,7]←M;
IF RCUBES[CUB,7]<0
THEN BEGIN RCUBES[CUB,11]←-RCUBES[CUB,5]; RCUBES[CUB,12]←RCUBES[CUB,4]; END
ELSE BEGIN RCUBES[CUB,11]←RCUBES[CUB,5]; RCUBES[CUB,12]←-RCUBES[CUB,4]; END;
FIND_HAND(DUM,DUM,DUM,DUM,XER,YER,TRUE);
IF STKFLG≠0 THEN RETURN;
KHC: WID←RCUBES[CUB,6]/2; HIT←3*WID/2-RHAND[2];
ARMTRAN(WID-RHAND[1],-WID,HIT,XT1,YT1,ZTH);
ARMTRAN(-WID-RHAND[1],-WID,HIT,XT2,YT2,ZTH);
IF RCUBES[CUB,7]≤0
THEN BEGIN ARMTRAN(-WID-RHAND[1],WID,HIT,XT3,YT3,ZTH);
XT1↔XT2; YT1↔YT2; END
ELSE ARMTRAN(WID-RHAND[1],WID,HIT,XT3,YT3,ZTH);
IF DBUG THEN OUTSTR(CVG(XT2)&CVG(YT2)&CVG(XT1)&CVG(YT1)&
CVG(XT3)&CVG(YT3)CRLF); WAIT;
MESS←ISSUE(7,"DRV","CAM",MESSAGE CAM_CENTER(CAMLENS,XT1/2+(XT2+XT3)/4,
YT1/2+(YT2+YT3)/4,ZTH));
IF CAMFLG≠0 THEN BEGIN STKFLG←4; RETURN; END;
IMAGECOR(XT1,YT1,ZTH,XIC,YIC); IMAGECOR(XT2,YT2,ZTH,XIB,YIB);
IMAGECOR(XT3,YT3,ZTH,XID,YID);
LNORML(XIB,YIB,XIC,YIC,DIRD1); LNORML(XIC,YIC,XID,YID,DIRD2);
IF ABS(DIRD1[1])<.707
THEN BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END
ELSE BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END;
LOOK_AT[2]←DIRD1[5]+XER; LOOK_AT[3]←DIRD1[6]+YER; DIRD1[8]←3;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(1,0,-9,.05,DIRD1));
IF DIR_EYE[0,1]<2 THEN
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(-1,0,-9,.05,DIRD1));
IF ((KMAX←(DIR_EYE[0,1]-1))<1)∨(KMAX>5)
THEN BEGIN OUTSTR("PLAS_CUBE-FAILED: 1ST. EDGE DISAPPEARED"CRLF);
STKFLG←8; RETURN; END;
IF KMAX=1
THEN ARRBLT(DIRD3[1],DIR_EYE[1,1],8)
ELSE BEGIN INTEGER ORD; ORD←0;
FOR K←1 STEP 1 UNTIL KMAX DO BEGIN
DIS[K]←(DIRD1[5]-DIR_EYE[K,5])*DIRD1[2]
-(DIRD1[6]-DIR_EYE[K,6])*DIRD1[1];
IND[K]←K; END;
WHILE ORD=0 DO BEGIN ORD←1;
FOR K←2 STEP 1 UNTIL KMAX DO
IF DIS[K]<DIS[K-1] THEN BEGIN
DIS[K]↔DIS[K-1]; IND[K]↔IND[K-1]; ORD←0; END;
END;
ARRBLT(DIRD3[1],DIR_EYE[IND[1],1],8)
END;
FOR K←1 STEP 1 UNTIL 6 DO OUTSTR(CVG(DIRD3[K])); OUTSTR(""CRLF); WAIT;
IF DIRD3[7]=0 THEN LNORML(DIRD3[5],DIRD3[6],DIRD3[5]+DIRD3[2*DIRD3[8]-1],
DIRD3[6]+DIRD3[2*DIRD3[8]],DIRD3);
IF ABS(DIRD2[1])<.707
THEN BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END
ELSE BEGIN LOOK_AT[4]←18; LOOK_AT[5]←18; END;
LOOK_AT[2]←DIRD2[5]+XER; LOOK_AT[3]←DIRD2[6]+YER; DIRD2[8]←3;
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(1,0,-1,.05,DIRD2));
IF DIR_EYE[0,1]<2 THEN
MESS←ISSUE(7,"DRV","EYE",MESSAGE SRCH_IMAGE(-1,0,-1,.05,DIRD2));
IF DIR_EYE[0,1]≠2 THEN BEGIN
OUTSTR("PLAS_CUBE-FAILED: 2ND. EDGE DISAPPEARED"CRLF);
STKFLG←9; RETURN; END;
ARRBLT(DIRD4[1],DIR_EYE[1,1],8);
DUM←DIRD3[1]*DIRD4[2]-DIRD3[2]*DIRD4[1];
XIC←(DIRD3[1]*DIRD4[1]*(DIRD3[6]-DIRD4[6])+DIRD3[1]*DIRD4[2]*DIRD4[5]-
DIRD3[2]*DIRD4[1]*DIRD3[5])/DUM;
YIC←(DIRD3[2]*DIRD4[2]*(DIRD4[5]-DIRD3[5])+DIRD3[1]*DIRD4[2]*DIRD3[6]-
DIRD3[2]*DIRD4[1]*DIRD4[6])/DUM;
TABLECOR(XIC,YIC,XT1,YT1);
XT2←XT1-RCUBES[CUB,6]*(RCUBES[CUB,4]+RCUBES[CUB,11])/2;
YT2←YT1-RCUBES[CUB,6]*(RCUBES[CUB,5]+RCUBES[CUB,12])/2;
RCUBES[CUB,1]←XT2; RCUBES[CUB,2]←YT2; ICUBES[CUB,4]←2;
OUTSTR("PLAS_CUBE: CENTER=("&CVG(XT2)&CVG(YT2)&")"&
" ERROR=("&CVG(XTC-XT2)&CVG(YTC-YT2)&")"CRLF);
IF (ABS(XTC-XT2)<.1)∧(ABS(YTC-YT2)<.1) THEN RETURN; WAIT;
INC[1]←INC[2]←0; INC[3]←1.0; INC[4]←1;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_CHANGE(INC,.5,ARM_PLAN));
IF ¬ARM_PLAN THEN BEGIN
OUTSTR("PLAS_CUBE-FAILED: NO PLAN TO LIFT"CRLF);
STKFLG←5; RETURN; END;
ICUBES[CUB,2]←-1; ICUBES[CUB,4]←1; ICUBES[CUB,5]←1;
IF ARM_STATUS≠0 THEN BEGIN
OUTSTR("PLAS_CUBE-FAILED: NO LIFT (ARM_STATUS="
&CVOS(ARM_STATUS)&")"CRLF);
STKFLG←5; RETURN; END; WAIT;
INC[1]←XTC-XT2; INC[2]←YTC-YT2; INC[3]←0;
MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_CHANGE(INC,.1,ARM_PLAN));
IF (ARM_STATUS≠0)∨(¬ARM_PLAN) THEN BEGIN
OUTSTR("PLAS_CUBE-FAILED: NO CORRECTION (ARM_STATUS="
&CVOS(ARM_STATUS)&")"CRLF);
STKFLG←5; RETURN; END; WAIT;
PLACE(0); IF STKFLG≠0 THEN RETURN;
GOTO KHC;
END "PLAS_CUBE";
REAL XTC,YTC,DELM,DELS;
INTEGER FLAG,I,J,REQUEST,RLENS,BKGR,INT;
STRING COD;
LABEL TIT,TES,STUB;
PUT_DATA(0,0,"DRV");
OUTSTR("STKDRV-WAITING"CRLF);
WHILE (¬YES_CAM)∨(¬YES_EYE)∨(¬YES_HAND) DO CALL(1,"SLEEP");
OUTSTR("STKDRV-ACTIVATED"CRLF);
DBUG←FALSE; P0X←51.22; P0Y←47.98; MARKX←-.3; MARKZ←0; THICK←.55;
HOME_ARM; GCOR←GRASP_CAL;
TIT: OUTSTR("1-RESET ARRAY CUBES"CRLF&
"2-CHANGE THE LENS"CRLF&
"3-FIND A CUBE"CRLF&
"4-PLACE A CUBE"CRLF&
"5-STACK A CUBE"CRLF&
"6-CLEAR THE VIEW"CRLF&
"7-COMPLEMENT DBUC"CRLF);
TES: OUTSTR("...TYPE THE NO. OF THE PROCEDUE YOU WANT TO USE NOW="CRLF);
REQUEST←CVD(INCHWL);
IF (REQUEST<1)∨(REQUEST>7) THEN BEGIN
OUTSTR("TESTIT-FAILED: ILLEGAL PROCEDURE NO ("&CVS(REQUEST)&")"CRLF);
GOTO TES; END;
STKFLG←0;
CASE REQUEST OF BEGIN
BEGIN ⊃ 0; END;
BEGIN ⊃ 1; FOR I←1 STEP 1 UNTIL 8 DO ICUBES[I,4]←0; HCUBE←0; END;
BEGIN ⊃ 2; OUTSTR("...TYPE THE LENS NO. YOU WANT TO USE="CRLF);
RLENS←CVD(INCHWL);
MESS←ISSUE(7,"DRV","CAM",MESSAGE CHNG_LENS(RLENS)); END;
BEGIN ⊃ 3; OUTSTR("...TYPE NO. OF CUBE="CRLF); J←CVD(INCHWL);
OUTSTR("...TYPE X COR. OF CENTER="CRLF);
COD←INCHWL; XTC←REALSCAN(COD,FLAG);
OUTSTR("...TYPE Y COR. OF CENTER="CRLF);
COD←INCHWL; YTC←REALSCAN(COD,FLAG);
OUTSTR("...TYPE VALUE OF BKGR="CRLF); BKGR←CVD(INCHWL);
OUTSTR("...TYPE VALUE OF INT="CRLF); INT←CVD(INCHWL);
FIND_CUBE(J,BKGR,INT,XTC,YTC); END;
BEGIN ⊃ 4; OUTSTR("...TYPE NO. OF CUBE="CRLF); J←CVD(INCHWL);
OUTSTR("...TYPE X COR. OF CENTER="CRLF);
COD←INCHWL; XTC←REALSCAN(COD,FLAG);
OUTSTR("...TYPE Y COR. OF CENTER="CRLF);
COD←INCHWL; YTC←REALSCAN(COD,FLAG);
PLAS_CUBE(J,XTC,YTC); END;
BEGIN ⊃ 5; OUTSTR("...TYPE NO. OF TOP CUBE="CRLF); J←CVD(INCHWL);
OUTSTR("...TYPE NO. OF BOTTOM CUBE="CRLF); I←CVD(INCHWL);
OUTSTR("...TYPE DELM="CRLF); COD←INCHWL;
DELM←REALSCAN(COD,FLAG);
OUTSTR("...TYPE DELS="CRLF); COD←INCHWL;
DELS←REALSCAN(COD,FLAG);
STAK_CUBE(I,J,DELM,DELS); END;
BEGIN ⊃ 6; UNGRASP(3); IF STKFLG=0 THEN HOME_ARM; END;
BEGIN ⊃ 7; DBUG←¬DBUG; GOTO TES; END;
END; WAIT;
OUTSTR(CVS(HCUBE)CRLF);
FOR I←1 STEP 1 UNTIL 8 DO
IF ICUBES[I,4]≠0 THEN BEGIN
OUTSTR(CVS(I));
FOR J←1 STEP 1 UNTIL 10 DO OUTSTR(CVG(RCUBES[I,J]));
FOR J←1 STEP 1 UNTIL 5 DO OUTSTR(" "&CVS(ICUBES[I,J]));
OUTSTR(""CRLF); END;
STUB: OUTSTR("...TYPE Y TO TEST AGAIN, OR EXIT:"CRLF); COD←INCHWL;
IF COD="Y"
THEN GOTO TIT
ELSE IF COD≠"EXIT" THEN BEGIN CLRBUF; GOTO STUB; END;
HOME_ARM;
END "STKDRV";
IF ABS(DIRD1[1])<.707
THEN BEGIN LOOK_AT[4]←36; LOOK_AT[5]←18; END
ELSE BEGIN LOOK_AT[4]←18; LOOK_AT[5]←36; END;